using BepuPhysics;
using BepuPhysics.Collidables;
using BepuPhysics.Constraints;
using BepuUtilities;
using DemoContentLoader;
using DemoRenderer;
using DemoRenderer.UI;
using DemoUtilities;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace Demos.Demos
{
    /// <summary>
    /// 展示了几种不同的制作绳子的方法,把重物绑在一起,而不是吓坏了。
    /// </summary>
    public class RopeStabilityDemo : Demo
    {
        static BodyHandle[] BuildRopeBodies(Simulation simulation, in Vector3 start, int bodyCount, float bodySize, float bodySpacing, float massPerBody, float inverseInertiaScale)
        {
            BodyHandle[] handles = new BodyHandle[bodyCount + 1];
            var ropeShape = new Sphere(bodySize);
            ropeShape.ComputeInertia(massPerBody, out var ropeInertia);
            Symmetric3x3.Scale(ropeInertia.InverseInertiaTensor, inverseInertiaScale, out ropeInertia.InverseInertiaTensor);
            var ropeShapeIndex = simulation.Shapes.Add(ropeShape);
            // 建立链接。
            var bodyDescription = new BodyDescription
            {
                // 使最上面的挡路处于运动状态,以支撑链条的睡觉。
                Activity = new BodyActivityDescription(.01f),
                Collidable = new CollidableDescription(ropeShapeIndex, 0.1f),
            };
            for (int linkIndex = 0; linkIndex < bodyCount + 1; ++linkIndex)
            {
                bodyDescription.LocalInertia = linkIndex == 0 ? new BodyInertia() : ropeInertia;
                bodyDescription.Pose = new RigidPose(start - new Vector3(0, linkIndex * (bodySpacing + 2 * bodySize), 0));
                handles[linkIndex] = simulation.Bodies.Add(bodyDescription);
            }

            return handles;
        }
        public static BodyHandle[] BuildRope(Simulation simulation, in Vector3 start, int bodyCount, float bodySize, float bodySpacing, float constraintOffsetLength, float massPerBody, float inverseInertiaScale, SpringSettings springSettings)
        {
            var handles = BuildRopeBodies(simulation, start, bodyCount, bodySize, bodySpacing, massPerBody, inverseInertiaScale);
            var maximumDistance = 2 * bodySize + bodySpacing - 2 * constraintOffsetLength;
            for (int i = 0; i < handles.Length - 1; ++i)
            {
                simulation.Solver.Add(handles[i], handles[i + 1],
                    new DistanceLimit(new Vector3(0, -constraintOffsetLength, 0), new Vector3(0, constraintOffsetLength, 0), maximumDistance * 0.1f, maximumDistance, springSettings));
            }
            return handles;
        }

        static BodyHandle CreateWreckingBall(Simulation simulation, BodyHandle[] bodyHandles, float ropeBodyRadius, float bodySpacing, float wreckingBallRadius, BodyInertia wreckingBallInertia, TypedIndex wreckingBallShapeIndex)
        {
            var lastBodyReference = new BodyReference(bodyHandles[bodyHandles.Length - 1], simulation.Bodies);
            var wreckingBallPosition = lastBodyReference.Pose.Position - new Vector3(0, ropeBodyRadius + bodySpacing + wreckingBallRadius, 0);
            var description = BodyDescription.CreateDynamic(wreckingBallPosition, wreckingBallInertia, new CollidableDescription(wreckingBallShapeIndex, 0.1f), new BodyActivityDescription(0.01f));
            // 稍微隆起一下。
            description.Velocity = new BodyVelocity(new Vector3(-10, 0, 0), default);
            var wreckingBallBodyHandle = simulation.Bodies.Add(description);
            return wreckingBallBodyHandle;
        }

        public static BodyHandle AttachWreckingBall(Simulation simulation, BodyHandle[] bodyHandles, float ropeBodyRadius, float bodySpacing, float constraintOffsetLength, float wreckingBallRadius, BodyInertia wreckingBallInertia, TypedIndex wreckingBallShapeIndex, SpringSettings springSettings)
        {
            BodyHandle wreckingBallBodyHandle = CreateWreckingBall(simulation, bodyHandles, ropeBodyRadius, bodySpacing, wreckingBallRadius, wreckingBallInertia, wreckingBallShapeIndex);
            var maximumDistance = bodySpacing + ropeBodyRadius - constraintOffsetLength;
            simulation.Solver.Add(bodyHandles[bodyHandles.Length - 1], wreckingBallBodyHandle,
                new DistanceLimit(new Vector3(0, -constraintOffsetLength, 0), new Vector3(0, wreckingBallRadius, 0), maximumDistance * 0.1f, maximumDistance, springSettings));
            return wreckingBallBodyHandle;
        }

        RolloverInfo rolloverInfo;


        public unsafe override void Initialize(ContentArchive content, Camera camera)
        {
            camera.Position = new Vector3(0, 25, 80);
            camera.Yaw = 0;
            camera.Pitch = 0;

            // 本演示的大部分内容是在不对模拟配置进行重大更改的情况下解决行为问题。
            // 如果您愿意更改模拟配置,则以下许多技巧都是完全不必要的。例如,尝试使用其中一个解算器子步进计时器。
            // 下面每个主时间步采用4个姿势积分/解算器子步骤-在多个帧上有效地共享碰撞检测和其他记账。
            // 请注意,解算器速度迭代次数降为1,因此每个解算器子步的开销也低于常规解算器执行。
            // 我们可以逃脱惩罚,因为提高更新率是到目前为止稳定模拟最有效的方法。
            // 事实上,在特别困难的模拟中,增加更新率、删除其他稳定的解决办法以及减少求解器迭代计数实际上可以"更快"。
            // 在此模拟中,使用4个子步并进行1次速度迭代,每个子步的成本比使用8次速度迭代的非子步进版本高出约25%。对质量的提高来说还不错。
            // 还要注意的是,这两种仿真配置都使用了比通常演示的接触刚度更高的接触刚度。那只是为了让你能把绳子缠绕在附近的太空舱上。
            // 在具有大量堆叠的模拟中,高接触刚度将需要分步或更高的更新率以保持稳定性。
            // Simulation=Simulation.Create(BufferPool,new DemoNarrowPhaseCallback(){ContactSpringness=new SpringSettings(120,1)},new DemoPoseIntegratorCallback(new Vector3(0,-10,0)),new SubsteppingTimestpper(4),1);

            // 因此,即使您可以避免这些类型的黑客攻击,如果您发现自己处于不可行的分步执行的环境中,知道它们存在也是件好事。
            Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks() { ContactSpringiness = new SpringSettings(120, 1) }, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());

            rolloverInfo = new RolloverInfo();
            var smallWreckingBall = new Sphere(1);
            smallWreckingBall.ComputeInertia(5, out var smallWreckingBallInertia);
            var smallWreckingBallIndex = Simulation.Shapes.Add(smallWreckingBall);
            {
                // 在建造绳索时,您可以尝试的第一件事是一个由小质量物体组成的简单链条,它们通过距离限制连接在一起,并以一种自然的方式将破坏球固定在末端。
                var startLocation = new Vector3(-55, 35, 0);
                const float bodySpacing = 0.3f;
                const float bodyRadius = 0.5f;
                var springSettings = new SpringSettings(30, 1);
                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 1, 1, springSettings);

                // 对于一个小的破碎球,这实际上在合理的弹簧设置下工作得很好。
                AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, smallWreckingBall.Radius, smallWreckingBallInertia, smallWreckingBallIndex, springSettings);
                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "Naive, 5:1 mass ratio");
            }
            var bigWreckingBall = new Sphere(3);
            // 这个破碎球要重得多。
            bigWreckingBall.ComputeInertia(100, out var bigWreckingBallInertia);
            var bigWreckingBallIndex = Simulation.Shapes.Add(bigWreckingBall);
            {
                // 一切都和第一根绳子一样,但现在有了一个更重的破碎球。
                // 这将会表现得非常糟糕。这个极其沉重的破碎球依赖于重量轻得多的绳体。
                // 保持破碎球就位所需的力需要通过绳索传播到顶部的运动学上,但是绳索的质量很小。
                // 这使得这一过程非常耗时。
                // 要在不更改约束配置的情况下稳定此情况,需要荒谬的求解器迭代次数或增加更新速率。
                var startLocation = new Vector3(-35, 35, 0);
                const float bodySpacing = 0.3f;
                const float bodyRadius = 0.5f;
                var springSettings = new SpringSettings(30, 1);
                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 1, 1, springSettings);

                AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "Naive, 100:1 mass ratio");
            }
            {
                // 如果刚度有问题,我们通过调整弹簧设置频率来降低刚度怎么样？
                // 这当然使它更稳定,但它的行为更像松散的弹性,而不是绳子。
                // 如果您有一个模拟,其中柔和度实际上是可以接受的,则这通常是最快、最简单的修复方法。
                var startLocation = new Vector3(-15, 35, 0);
                const float bodySpacing = 0.3f;
                const float bodyRadius = 0.5f;
                var springSettings = new SpringSettings(3, 1);
                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 1, 1, springSettings);

                AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "Softer constraints");
            }
            {
                // 如果破碎球和绳体之间的质量比使脉冲很难传播,那么增加绳索的质量又如何呢？
                // 它确实有帮助,但是现在绳子真的很重。这并不是我们真正想要的行为,它仍然不是完美的。
                var startLocation = new Vector3(-5, 35, 0);
                const float bodySpacing = 0.3f;
                const float bodyRadius = 0.5f;
                var springSettings = new SpringSettings(30, 1);
                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 20, 1, springSettings);

                AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "20x rope mass boost");
            }
            {
                // 传播冲动的困难不仅仅是质量问题。约束杆臂和身体惯性张量也很重要。
                // 不如我们把物体看作比它们的形状所暗示的要大得多的转动惯量,而不要增加质量那么多怎么样？
                // 这和使用较高的质量一样有帮助,但对行为没有太大的负面影响。
                var startLocation = new Vector3(5, 35, 0);
                const float bodySpacing = 0.3f;
                const float bodyRadius = 0.5f;
                var springSettings = new SpringSettings(30, 1);
                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, bodyRadius, 5, 0.2f, springSettings);

                AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bodyRadius, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "5x rope mass boost, 25x rope inertia boost");
            }
            {
                // 如果增加惯性有帮助,并且更长的杠杆臂可以使问题更难解决,那么将约束杠杆臂减少到零怎么样？
                // 所以绳体的角运动一点也不重要？
                // (我们还将通过将绳体的转动惯量设置为无穷大来锁定绳体方向,但由于约束杆臂的长度为0,这实际上并不
                // 改变一切。)
                // 这很有帮助。角振荡被完全消除,强迫线性振荡需要相当多的时间。
                var startLocation = new Vector3(15, 35, 0);
                const float bodySpacing = 0.3f;
                const float bodyRadius = 0.5f;
                var springSettings = new SpringSettings(30, 1);
                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, 0, 1, 0, springSettings);

                AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, 0, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "0 lever arm");
            }
            {
                // 但是如果我们不想改变绳体的质量,但是我们想要比零杠杆臂更好呢？
                // 还有一个选择！将破碎球直接连接到运动学上。不用再担心脉冲传播了。
                // 这使得破碎球在自由摆动时,绳索的睡觉有效美容。
                // 问题是,你可以"看得出来"这根绳子是用来装饰的。它没装子弹,所以翻得太厉害了。
                // 此外,如果绳索缠绕在某个东西上,以致作弊约束不再支撑重量,绳索将会像第一次尝试时一样吓坏。
                var startLocation = new Vector3(25, 35, 0);
                const float bodySpacing = 0.3f;
                const float bodyRadius = 0.5f;
                var springSettings = new SpringSettings(30, 1);
                var bodyHandles = BuildRope(Simulation, startLocation, 12, bodyRadius, bodySpacing, 0, 1f, 0, springSettings);

                var wreckingBallHandle = AttachWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, 0, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex, springSettings);
                var wreckingBallConnectionOffset = new Vector3(0, bigWreckingBall.Radius, 0);
                var maximumDistance = Vector3.Distance(
                    new BodyReference(bodyHandles[0], Simulation.Bodies).Pose.Position,
                    new BodyReference(wreckingBallHandle, Simulation.Bodies).Pose.Position + wreckingBallConnectionOffset);
                Simulation.Solver.Add(bodyHandles[0], wreckingBallHandle, new DistanceLimit(default, wreckingBallConnectionOffset, 0.01f, maximumDistance, springSettings));
                rolloverInfo.Add(startLocation + new Vector3(0, 2, 0), "0 lever arm, direct cheat constraint");
            }
            {
                // 上一次尝试相当稳定,但是我们如何解决它的问题呢？与从源到目标的单个约束不同,我们可以
                // 在附近的绳体之间创建一串"跳跃约束"。不必一次遍历约束图的一个主体,
                // 这些跳过约束允许脉冲同时沿着多条"快捷"路径传播通过图形。任何一个物体都会成为瓶颈
                // 使其负载快速传播到其所有邻居。
                // 因此,这是非常稳定的。您可以选择增加跳过约束的数量以获得更高的稳定性。进一步减小绳索质量是很有可能的。
                var startLocation = new Vector3(35, 140, 0);
                const float bodySpacing = 0.3f;
                const float bodyRadius = 0.5f;
                var springSettings = new SpringSettings(30, 1);
                var bodyHandles = BuildRopeBodies(Simulation, startLocation, 100, bodyRadius, bodySpacing, 1f, 0);

                bool TryCreateConstraint(int handleIndexA, int handleIndexB)
                {
                    if (handleIndexA >= bodyHandles.Length || handleIndexB >= bodyHandles.Length)
                        return false;
                    var maximumDistance = Vector3.Distance(
                        new BodyReference(bodyHandles[handleIndexA], Simulation.Bodies).Pose.Position,
                        new BodyReference(bodyHandles[handleIndexB], Simulation.Bodies).Pose.Position);
                    Simulation.Solver.Add(bodyHandles[handleIndexA], bodyHandles[handleIndexB], new DistanceLimit(default, default, .01f, maximumDistance, springSettings));
                    return true;
                }
                const int constraintsPerBody = 4;
                for (int i = 0; i < bodyHandles.Length - 1; ++i)
                {
                    // 请注意,您还可以创建跨越更多链接的约束。例如,连接i和i+1、i+2、i+4、i+8和i+16,而不仅仅是最近的实体。
                    // 这将使它的行为更像前面的作弊约束,但它可能会很有用。
                    for (int j = 1; j <= constraintsPerBody; ++j)
                    {
                        if (!TryCreateConstraint(i, i + j))
                            break;
                    }
                }

                var wreckingBallHandle = CreateWreckingBall(Simulation, bodyHandles, bodyRadius, bodySpacing, bigWreckingBall.Radius, bigWreckingBallInertia, bigWreckingBallIndex);
                var wreckingBallConnectionOffset = new Vector3(0, bigWreckingBall.Radius, 0);
                for (int i = 1; i <= constraintsPerBody; ++i)
                {
                    var targetBodyHandleIndex = bodyHandles.Length - i;
                    if (targetBodyHandleIndex < 0)
                        break;
                    var maximumDistance = Vector3.Distance(
                        new BodyReference(bodyHandles[targetBodyHandleIndex], Simulation.Bodies).Pose.Position,
                        new BodyReference(wreckingBallHandle, Simulation.Bodies).Pose.Position + wreckingBallConnectionOffset);
                    Simulation.Solver.Add(bodyHandles[targetBodyHandleIndex], wreckingBallHandle, new DistanceLimit(default, wreckingBallConnectionOffset, 0.01f, maximumDistance, springSettings));
                }
                rolloverInfo.Add(startLocation, $"100:1 mass ratio, {constraintsPerBody - 1}x extra skip constraints");
            }

            Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(200, 1, 200)), 0.1f)));
            Simulation.Statics.Add(new StaticDescription(
                new Vector3(100, 70, 0), BepuUtilities.QuaternionEx.CreateFromAxisAngle(new Vector3(1, 0, 0), System.MathF.PI * 0.5f),
                new CollidableDescription(Simulation.Shapes.Add(new Capsule(8, 64)), 0.1f)));

        }

        public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
        {
            rolloverInfo.Render(renderer, camera, input, text, font);
            base.Render(renderer, camera, input, text, font);
        }

    }
}
